package edu.uhcl.team_drone.drone;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector3;

public class MovementComponent {
    // constants
    private final float MAX_THROTTLE = 200;
    private final float MAX_CONTROL = 70;
    private final float TILT_FACTOR = 1/100f;
    private final float TILT_TO_MOVEMENT_FACTOR = 11;
    private final int ACCELERATION = 5;    
    private final Vector3 TRUE_UP = new Vector3(0, 1, 0);
       
    
    private float controlX, controlY, controlZ;
    private float dx, dy, dz;

    private Vector3 position = new Vector3(); // Current Position
    private Vector3 up = TRUE_UP;     // upwards from the current position
    private Vector3 direction = new Vector3(1, 0, 0); // forward direction from current position   
    private Vector3 temp = new Vector3();
    private Vector3 right = new Vector3();
    
    private boolean onGround = false;

    private final Drone owner;
    

    public MovementComponent(Drone ownerIn, Vector3 startingPositionIn) {
        this.owner = ownerIn;
        this.position = startingPositionIn;
        controlX = controlY = controlZ = 0;
        dx = dy = dz = 0;
    }

    public void update(float dt) {
        // find right for vector math
        right = direction.cpy().crs(up).nor();        
        
        limitControlRates();
        tiltFromControls();
        moveDueToTilt(dt);
        moveInY();        
        zeroOutControls();
        dontGoThroughFloor();

        MathUtils.clamp(dx, -5.0f, 5.0f);
        MathUtils.clamp(dz, -5.0f, 5.0f);
        System.out.println("dx: " + dx + " , dz: " + dz);
        position.add(dx, 0, dz);

        // dampen y (GRAVITY)
        if (controlY > 0) {
            controlY -= 1.0f;
        }
    }

    private void limitControlRates() {
        if (controlX > MAX_CONTROL) {
            controlX = MAX_CONTROL;
        } else if (controlX < -MAX_CONTROL) {
            controlX = -MAX_CONTROL;
        }
        if (controlZ > MAX_CONTROL) {
            controlZ = MAX_CONTROL;
        } else if (controlZ < -MAX_CONTROL) {
            controlZ = -MAX_CONTROL;
        }
        //System.out.println(controlX + " " + controlZ);
    }

    private void tiltFromControls() {
        // TILT left / right
        up.rotate(direction, controlX * TILT_FACTOR);

        // TILT up / down
        up.rotate(right, -controlZ * TILT_FACTOR);

        // fix direction vector after tilts
        direction = up.cpy().crs(right).nor();

    }

    private void moveDueToTilt(float dt) {
        if (!onGround) {

            temp.set(up).nor();
            dx += temp.x * TILT_TO_MOVEMENT_FACTOR * dt;
            dz += temp.z * TILT_TO_MOVEMENT_FACTOR * dt;
//            position.add(
//                    temp.x * TILT_TO_MOVEMENT_FACTOR,
//                    temp.y - 1 * 3, // 3 is the movement factor in Y
//                    temp.z * TILT_TO_MOVEMENT_FACTOR);
        }
    }

    private void moveInY() {
        // move in Y(up)
        temp.set(up).nor().scl(Gdx.graphics.getDeltaTime() * controlY);
        //dy += temp.y - 1;
        position.add(temp);
    }

    private void zeroOutControls() {
        if (controlX > 0) {
            controlX -= 1;
        } else if (controlX < 0) {
            controlX += 1;
        }
        if (controlZ > 0) {
            controlZ -= 1;
        } else if (controlZ < 0) {
            controlZ += 1;
        }
     

    }

    private void dontGoThroughFloor() {
        if (position.y <= 40) {
            onGround = true;
            position.y = 40;
            controlX = controlZ = 0;            
            up = TRUE_UP;
            direction = up.cpy().nor().crs(right);
        }else{
            onGround = false;
        }        
    }

    public void move(DroneCommands.COMMAND commandIn) {
        switch (commandIn) {
            case UP:
                if (controlY < MAX_THROTTLE) {
                    controlY += ACCELERATION;
                }
                break;
            case DOWN:
                if (controlY > -MAX_THROTTLE) {
                    controlY -= ACCELERATION;
                }
                break;
            case LEFT:
                controlX -= ACCELERATION;
                break;
            case RIGHT:
                controlX += ACCELERATION;
                break;
            case FORWARD:
                controlZ += ACCELERATION;
                break;
            case BACKWARD:
                controlZ -= ACCELERATION;
                break;
            case ROTATE_RIGHT:
                direction.rotate(up, -1);
                up.rotate(up, -1);
                break;
            case ROTATE_LEFT:
                direction.rotate(up, +1);
                up.rotate(up, +1);
                break;
        }
    }

    public Vector3 getPosition() {
        return position;
    }

    public Vector3 getDirection() {
        return direction;
    }

    public Vector3 getUp() {
        return up;
    }

}
